#ifndef OMPL_GEOMETRIC_PLANNERS_APPROX_CSPACE_COLLISION_ESTIMATOR_H
#define OMPL_GEOMETRIC_PLANNERS_APPROX_CSPACE_COLLISION_ESTIMATOR_H

#include <hplanner_ompl/approx_cspace_oracle.h>
#include <hplanner_ompl/reasoning.h>



namespace ompl
{
namespace base
{

class CollisionStatusPointEstimator
{
public:
  typedef typename std::vector<std::pair<unsigned int, float> >  QueryResultType;

  CollisionStatusPointEstimator(ApproxCSpaceOracleBase* oracle, float obstacle_density = 1)
  {
    oracle_ = oracle;
    obstacle_density_ = obstacle_density;

    unsigned int dim = oracle_->getStateToVectorTransformer().dim();
  }

  ~CollisionStatusPointEstimator()
  {

  }

  void setOracle(ApproxCSpaceOracleBase* oracle)
  {
    oracle_ = oracle;
  }

  void setObstacleDensity(float obstacle_density)
  {
    obstacle_density_ = obstacle_density;
  }

  float computeCollisionProb(const State* s, bool* sufficient = NULL) const;

  float computeCollisionProbSample(const State* s1, const State* s2, bool* sufficient = NULL) const;

  float computeCollisionProbSample(const State* s1, const State* s2, std::vector<float>& probs, bool* sufficient = NULL) const;

protected:
  ApproxCSpaceOracleBase* oracle_;

  float obstacle_density_;

};

class CollisionStatusLineEstimator
{
public:
  typedef typename std::vector<std::pair<unsigned int, float> >  QueryResultType;

  CollisionStatusLineEstimator(ApproxCSpaceOracleBase* oracle, float obstacle_density = 1, float coherence_weight = 0.05)
  {
    oracle_ = oracle;
    obstacle_density_ = obstacle_density;
    coherence_weight_ = coherence_weight;

    unsigned int dim = oracle_->getStateToVectorTransformer().dim();
    classifier_svm_ = new cspace_learning::LinearClassifier_SVM(dim);
    classifier_bisect_ = new cspace_learning::LinearClassifier_Bisect(dim);
  }

  ~CollisionStatusLineEstimator()
  {
    delete classifier_svm_;
    delete classifier_bisect_;
  }

  void setOracle(ApproxCSpaceOracleBase* oracle)
  {
    oracle_ = oracle;
  }

  void setObstacleDensity(float obstacle_density)
  {
    obstacle_density_ = obstacle_density;
  }

  void setCoherenceWeight(float coherence_weight)
  {
    coherence_weight_ = coherence_weight;
  }


  float computeCollisionProbSample(const State* s1, const State* s2, bool* sufficient = NULL) const;

  float computeCollisionProbSample(const State* s1, const State* s2, std::vector<float>& probs, bool* sufficient = NULL) const;

  std::pair<float, float> computeCollisionSVM(const State* s1, const State* s2, bool* sufficient = NULL) const;

  float computeCollisionProbGen(const State* s1, const State* s2, bool* sufficient = NULL) const;

  std::pair<float, unsigned int> computeCollisionProbDP(const State* s1, const State* s2,
                                                        bool s1_collision, bool s2_collision,
                                                        bool* sufficient = NULL) const;

  std::pair<float, unsigned int> computeCollisionProbDP(const State* s1, const State* s2,
                                                        bool s1_collision, bool s2_collision,
                                                        std::vector<float>& probs,
                                                        bool* sufficient = NULL) const;

  float computeClosestInCollisionSampleDist(const State* s1, const State* s2) const;

  float getObstacleDensity() const
  {
    return obstacle_density_;
  }

protected:
  ApproxCSpaceOracleBase* oracle_;

  float obstacle_density_;

  float coherence_weight_;

  cspace_learning::LinearClassifier_SVM* classifier_svm_;

  cspace_learning::LinearClassifier_Bisect* classifier_bisect_;

  float computeCollisionProbSample_(const State* s, const QueryResultType& collision_result, const QueryResultType& collision_free_result) const;
};


}
}

#endif
